%% data = hybrid_flow(xi, di, num_execs)
%
%  Description:
%   Simulate flow of a hybrid system.
%
%  Operational Description:
%   Defines a state variable that keeps track of the domain in which the
%   system is operating, i.e. the discrete state.  Defines the guards,
%   reset maps and constraints for each discrete state.  Constraints are
%   stored in the form of A' * lambda.  Defines a data structure:  The
%   integral_curves member contains the solution to the different equation
%   for the interval listed in the hybrid_interval member.  Stores the 
%   discrete state variable 'current_state' as a member in the 'robot'
%   struct.  Then loops for 'num_execs' executions.  Each time through the
%   loop, stores the guards, reset maps and constraints associated with the
%   discrete state in robot.system and passes this to the hybrid_step()
%   function, which evaluates the ODE until the system hits a guard.
%   hybrid_step() returns the results of integration along with the next
%   discrete state of the system.  Then returns to the beginning of the
%   loop.
%
%  Arguments:
%   xi        - Initial condition of the continuous state.
%   di        - The initial discrete state.
%   num_execs - Specify the number of executions of the hybrid system.
%
%  Return Values:
%   data      - Struct with fields 'hybrid_interval' and 'integral_curves'
%               which specify the solutions to the various discrete states
%               and the time intervals over which these solutions are
%               valid.
%
%  Error Handling:
%   HybridSystem:ResetToGuard   -> This exception is thrown by
%                                  hybrid_step() when reset maps to a
%                                  location on the guard.  This could
%                                  result in Zeno.
%   HybridSystem:DidNotHitGuard -> This exception is thrown by
%                                  hybrid_step() when the system does not
%                                  hit a guard in the maximum amount of
%                                  time allowed for each interval which is
%                                  defined in hybrid_step().
%
function data = hybrid_flow(xi, di, num_execs, lsiObj)
stepnum = 0;
wb = waitbar(0, 'Please wait...');
set(wb, 'Name', [num2str(0) '%']);

%% Check if the number of executions is valid.
%
if round(num_execs) ~= num_execs || num_execs < 1
    ME = MException('HybridSystem:InvalidExecutionNumber', ...
        'Number of executions must be a positive integer.');
    throw(ME);
end

%% Define the systems for the different domains.  Each domain has three
% transitions possible for this particular model.
%

% State 1 - Stance Toe Down, Knee Free
%
systems{1}.guards = @h_sca;
systems{1}.dguards = @(x) -1;
systems{1}.resets = @reset_map;
systems{1}.odefcn = @(t, x) f_tt_vector(t, x, lsiObj);
              
%% Initialize output data.
%
data.time = cell(num_execs, 1);
data.hybrid_interval = cell(num_execs, 1);
data.integral_curves = cell(num_execs, 1);
data.discrete_state = cell(num_execs, 1);

% ti is the starting time.  It is totally irrelevant to the operation of
% an autonomous system.
%
ti = 0;
%% Go through num_execs executions.  There is one reset per execution.
%
for i = 1 : num_execs
    
    
    if i == 5
        0;
    end
    
    %% Pass the correct system to the hybrid_step function.
    %
    robot.system = systems{1};
    
    %% Do one execution, passing the associated guards, maps, and
    % constraints.  xi is the initial condition.  robot.system contains the
    % guards, maps and constraints for the particular domain.
    %
    %     try
    execution = hybrid_step(xi, di, robot);
    
    %% Save the data from the execution, adding the end time of the
    % previous execution to the times returned.  Also construct a two-
    % element vector containing the interval over which this execution
    % takes place.
    %
    data.time{i} = ti + execution.time;
    data.integral_curves{i} = execution.integral_curve;
    data.discrete_state{i} = execution.discrete_state;
    data.hybrid_interval{i} = data.time{i}([1 end]);
    
    %% Save the total time elapsed.  Also save the position of the system
    % as the initial condition for the next step.
    %
    tx = execution.time(end);
    ti = ti + tx;
    xi = execution.next_xi;
    di = execution.next_di;
    
    %% Return the state data.
    %
    data.events(i) = execution.event;

    stepnum = stepnum + 1;
    set(wb, 'Name', [num2str(stepnum/num_execs*100,'%3.1f') '%']);
    waitbar(stepnum/num_execs,wb);
end

close(wb);
